#include <iostream>
#include <vector>
#include <boost/shared_ptr.hpp>
#include "ros/ros.h"
#include "string"
#include "stanley_controller.hpp"
#include "getPath.hpp"
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <mvccep_image_localization/base_localization.h>

using namespace std;

State veh_state;
bool isbegin = false;

void control_Callback(const mvccep_image_localization::base_localization::ConstPtr& msg)
{
    veh_state = State(msg->robot_4.pos.x, msg->robot_4.pos.y, msg->robot_4.angle, 0, 0);
    std::cout << "[" << veh_state.x << ", " << veh_state.y << ", " << veh_state.yaw << "]" << std::endl;
    isbegin = true;
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "experiment_node");
	ros::NodeHandle nh("~");
	ros::NodeHandle nh_sub;
	ros::Subscriber localization_sub = nh_sub.subscribe("base_localization", 1, control_Callback);

	vector<State> path;
	getPath(path);

	Stanley st(nh, path);
	ros::Rate loop_rate(10);
	while (ros::ok())
	{
        ros::spinOnce();
        if (isbegin)
        {
            if (!st.isArrived())
                st.loop(veh_state);
            else
                ROS_INFO("The vehicle has reached the destination!!!");            
        }
		loop_rate.sleep();
	}

	return 0;
}